import queue
import socket
import threading
import time
import lib.udp_server as udp_server
from lib.log_udp_server import DEBUG,get_current_ms
import lib.MavlinkParse as mavlink

TCP_AUTO_TEST_ENABLE = False
TCP_SEND_AUTO_ADDING = False
TCP_IMPORT_MAVLINK = False

### 设置服务器自动解析mavlink数据包
def set_mavlink_enable(t):
    global TCP_IMPORT_MAVLINK
    TCP_IMPORT_MAVLINK = t

### 开启自动回测功能
def set_auto_test_enable(t):
    global TCP_AUTO_TEST_ENABLE
    TCP_AUTO_TEST_ENABLE = t

### 设置自动添加前缀发送到TCP客户端功能
def set_tcp_adding(d):
    global TCP_SEND_AUTO_ADDING
    TCP_SEND_AUTO_ADDING = d

### 将UDP服务器接收到的数据通过TCP下发到设备端
def tcp_server_send_task(tcp):
    last_msg_time_ms = get_current_ms()
    send_count = 0

    while True:
        if tcp.current_socket is None:
            time.sleep(0.005)
            continue
        
        conn = tcp.current_socket[0]
        addr = tcp.current_socket[1]
        now = get_current_ms()

        # 将UDP服务器接收到的数据通过TCP服务器发送出去
        while (not udp_server.udp_rec_queue.empty()):
            try:
                data = udp_server.udp_rec_queue.get()[2]
                if len(data)<=0:
                    continue
                
                sdata = data
                if TCP_SEND_AUTO_ADDING:
                    sdata = bytes("^IOTDATA=%d," % len(data), 'latin1') + data + bytes("\r\n", 'latin1')                
                conn.sendto(sdata, addr)

                send_count = send_count + len(data)
                if now>last_msg_time_ms and now-last_msg_time_ms>=5000:
                    msg = ">>> TCP Server -> DEV (%d / 5s)" % (send_count)
                    DEBUG(msg, True, True)
                    last_msg_time_ms = get_current_ms()
                    send_count = 0

            except OSError:
                DEBUG("APP-->TCP Server Send Error!!!", True, True)

        time.sleep(0.005)

### 将TCP服务器接收到的数据缓存，将来使用UDP服务器转发出去
def tcp_accept_process_task(tcp):

    #当前无连接，又有新连接进来，则开始运行
    if (tcp.current_socket is None) and (tcp.new_socket is not None):
        tcp.current_socket = tcp.new_socket
        td = threading.Thread(target=tcp_server_send_task, args=(tcp,))
        td.start()
    else:
        DEBUG("APP-->TCP Client Error!!!", True, True)
        return

    last_msg_time_ms = get_current_ms()
    rec_count = 0

    DEBUG("APP-->TCP Server Accept Process Running...", True, True)
    while True:

        if tcp.need_switch_socket():
            tcp.current_socket = tcp.new_socket
            tcp.switch_socket_pass()

        if tcp.current_socket is None:
            time.sleep(0.05)
            continue
        
        conn = tcp.current_socket[0]
        addr = tcp.current_socket[1]

        now = get_current_ms()
        try:
            data = conn.recv(4096)
            if TCP_AUTO_TEST_ENABLE:
                sdata = data
                if TCP_SEND_AUTO_ADDING:
                    sdata = bytes("^IOTDATA=%d," % len(data), 'latin1') + data + bytes("\r\n", 'latin1')                
                conn.sendto(sdata, addr)
        except OSError:
            conn.close()
            tcp.current_socket = None
            DEBUG("APP-->TCP Server Send Error!!!", True, True)

        if not data:
            conn.close()
            tcp.current_socket = None
        
        if len(data)<=0:
            continue

        #tcp.socket_rec_queue.put((conn, addr, data))
        #将TCP服务器接收到的数据缓存到UDP服务器发送缓冲区，待UDP发送至客户端
        udp_server.udp_send_queue.put((conn, addr, data))

        rec_count = rec_count + len(data)
        if now>last_msg_time_ms and now-last_msg_time_ms>=5000:
            msg = ">>> DEV -> TCP Server (%d / 5s)" % (rec_count)
            DEBUG(msg, True, True)
            last_msg_time_ms = get_current_ms()
            rec_count = 0
        
        if TCP_IMPORT_MAVLINK:
            m = tcp.mav.parse(data)
            if m is not None:
                msg = ">>> DEV -> TCP Server (mavlink): %s->(id=%d, len=%d)" % (m.name, m.get_header().msgId, m.get_header().mlen)
                DEBUG(msg, True, True)

def tcp_server_main_task(tcp):
    DEBUG("APP-->TCP server start listen:---", True, True)
    while True:
        conn, addr = tcp.tcp_server.accept()
        tcp.new_socket = (conn, addr)

        try:
            if tcp.current_socket is not None:
                tcp.current_socket[0].close()
                addr = tcp.current_socket[1]
                DEBUG("APP-->TCP server close(%s->%d)" % (addr[0],addr[1]), True, True)
        except:
            pass

        if tcp.sig_thread.is_alive():
            tcp.set_restart_thread()
        
        msg = "APP-->New TCP Connected by(%s->%d)" % (addr[0],addr[1])
        DEBUG(msg, True, True)

        # 发现新的TCP客户端，将UDP中缓存的数据清空
        udp_server.udp_rec_queue.queue.clear()
        udp_server.udp_send_queue.queue.clear()
        if not tcp.sig_thread.is_alive():
            tcp.sig_thread = threading.Thread(target=tcp_accept_process_task, args=(tcp,))
            tcp.sig_thread.start()

class TCP_Server:
    def __init__(self, host, port, maxconn=1, block=True):
        self.host = host
        self.port = port
        self.tcp_server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.tcp_server.setblocking(block)
        self.tcp_server.bind((host, port))
        if maxconn>0:
            self.maxconn = maxconn
        else:
            self.maxconn = 1
        self.tcp_server.listen(self.maxconn)
        self.socket_rec_queue = queue.Queue(0)
        self.sig_thread = threading.Thread()
        self.mav = mavlink.MavlinkParse()

        self.need_exit_pre_socket = False
        self.current_socket = None
        self.new_socket = None

    def set_restart_thread(self):
        self.need_exit_pre_socket = True
    
    def need_switch_socket(self):
        return self.need_exit_pre_socket

    def switch_socket_pass(self):
        self.need_exit_pre_socket = False

    def run(self, tcp):
        server_main_td = threading.Thread(target=tcp_server_main_task, args=(tcp,))
        server_main_td.start()
